/* Copyright (c) 2011 Alexandre Défossez, Timothée Lacroix
 * This file is part of HINAPT.
 *
 * HINAPT is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * HINAPT is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with HINAPT.  If not, see <http://www.gnu.org/licenses/>.
 */

 /*!
  \file AffineTracker.hpp
  Implémentation d'un affine tracker tel que décrit
  par Shi et Tomasi dans Good Feature To Track (1994)
 */

#ifndef AFFINETRACKER_HPP
#define AFFINETRACKER_HPP

#include "_tracker.hpp"

/*!
  Permet de suivre un objet avec une modélisation affine de son évolution
*/
class AffineTracker
{
  public:
    /*!
      \param n nombre maximum d'itération dans la méthode de Newton
      \param threshold seuil permettant de s'arrêter avant n itération
    */
    AffineTracker(int n=10, double threshold=0.1);
    /*!
      Calcul le mouvement entre la première et la nouvelle image
      \param img la nouvelle image
      \param[out] D la matrice de déformation (2x2)
      \param[out] d le vecteur de translation
      \param hint un indice de translation pour le tracker, il s'agit du point
      autour duquel l'objet doit se trouver sur la nouvelle image
      \return l'erreur quadratique
    */
    double track(Mat &img, Mat &D, Vec2d &d, Point2f hint);
    /*!
      Calcul le mouvement mais ne retourne que l'erreur quadratique
      \see track
      \param img
      \param hint
      \return l'erreur quadratique
    */
    double error(Mat &img, Point2f hint);
    /*!
      Retourne la première image
      \param[out] m
    */
    void getOriginal(Mat &m);
    /*!
      Retourne la position de l'objet sur la première image
    */
    Point2f getPoint();
    /*!
      Met à jour l'image et la position de l'objet
      \param img la nouvelle première image
      (uniquement la fenêtre autour de l'objet)
      \param point la position de l'objet sur la nouvelle image
    */
    void update(Mat &img, Point2f point);
    /*!
      Valeur retournée en cas d'erreur lors du tracking
    */
    static const double TRACK_FAILED;

  private:
    /*!
      nombre maximum d'itération dans la méthode de Newton
    */
    int n;
    /*!
      threshold seuil permettant de s'arrêter avant n itération
    */
    double threshold;
    /*!
      L'image de référence
    */
    Mat original;
    /*!
      Position de l'objet sur l'image de référence
    */
    Point p;

    /*! Une certaine notion d'intensité, ici la norme 1 au carré de l'image.
    Utilisé pour rendre indépendant l'erreur d'autres paramètres
    (taille, luminosité).
    */
    double intensity;


    /*!
      Calcul la matrice T à partir du gradient
      \param[in] grad la matrice du gradient spatial
      \param[out] T la matrice T
    */
    void calcT(Mat &grad, Mat &T);
    /*!
      Calcul la matrice a à partir de la nouvelle image et du gradient
      (et de l'image de référence)
      \param[in] img la nouvelle image
      \param[in] grad la matrice de gradient spatial
      \param[out] a la matrice a
    */
    void calcA(Mat &img, Mat &grad, Mat &a);
    /*!
      Fonction interne qui effectue la méthode de Newton
      \param[in] img la nouvelle image
      \param[in,out] A matrice l'application affine
      \param[in,out] d vecteur de translation
      \return l'erreur quadratique finale
    */
    double internalTrack(Mat &img, Mat &A, Mat &d);
    /*!
      Calcul l'erreur sur la fenêtre donnée
      \param[in] window la fenêtre sur laquelle l'erreur est calculée
      \return l'erreur quadratique
    */
    double calcErr(Mat &window);
    /*!
      Applique une transformation affine à une image sur une fenêtre
      \param[in] img image dont on doit extraire la sous-image
      \param[in,out] window la fenêtre d'extraction, contient la sous-image
      après appel
      \param[in] A matrice l'application affine
      \param[in] d vecteur de translation
      \return true si l'opération a réussi (i.e, on n'a pas dû à accéder
      à un pixel or de l'image)
    */
    bool extractWindow(Mat &img, Mat &window, Mat &A, Mat &d);
    /*!
      Applique une transformation affine aux 4 coins de la fenêtre
      \param[in] A matrice l'application affine
      \param[in] d vecteur de translation
      \param[in,out] la position des 4 coins
    */
    void windowMovement(Mat &A, Mat &d, Point2f pts[]);
    /*!
      Permet de vérifier la convergence de la méthode Newton et
      de l'arrêter
      \param[in] lastWindowPos la position des 4 coins de l'ancienne fenêtre
      \param[in] windowPos la position des 4 coins de la nouvelle fenêtre
      \return true s'il y a convergence (i.e les coins ont tous bougés
      de moins que threshold)
    */
    bool convergence(Point2f lastWindowPos[], Point2f windowPos[]);
};

/*!
  Applique une transformation affine à un point.
  \param p point auquel on applique la transformation
  \param[in] A matrice l'application affine
  \param[in] d vecteur de translation
  \return l'image de p par cette transformation affine
*/
inline Point2f applyAffineTransform(Point2f p, Mat &A, Mat &d){
  Mat tmp(2, 1, CV_64FC1);
  Mat res;
  tmp.at<double>(0, 0) = p.x;
  tmp.at<double>(1, 0) = p.y;
  res = A*tmp + d;
  return Point2f(res.at<double>(0, 0), res.at<double>(1, 0));

}



#endif // AFFINETRACKER_HPP
